/*
 * Implementation of the new corner detection algorithm
 * Original version available at http://www.cs.vsb.cz/sojka/cordet/presentation.html
 * Original author doc. Dr. Ing. Eduard Sojka 
 * Corrections made by Lukáš Tvrdý
 * VSB Technical University Ostrava 2008
 * 
 *  
 * */

#include <stdlib.h>
#include <stdio.h>
#include <math.h>
#include "cordet.h"

const float PIf = 3.1415926f;
const float TWO_PIf = 6.2831853f;
const float HALF_PIf = 1.5707963f;
const float MIN_APPAR = -999.9f;


/* The points whose Psg(X) is less than
 * this value are excluded from further processing
 * since they have almost no influence.  */
const float SMALL_PROBABILITY = 0.05f;

/* The function pd() is tabulated with the step 
 * 1.0/this value.                              */
const int PD_TABLE_SCALE = 250;    
                              
/* Minimal number of points X that are required 
 * for carrying out the measurement at Q.       */
const int SMALL_NUM_PIXELS = 3;


/* Theoretically, the angle between the corner axis and the directions of the gradient at   
points in the corner area cannot be greater than pi/2. Practically, the algorithm does   
not use this theoretical step function and uses a ramp function instead. The width of   
the ramp is defined by this value.   */
const float ANGLE_RAMP_WIDTH = 0.35f;


/*
 * Computes the neighbMap. Each item of this map incremented by imgPosQ points
 * to the appropriate position in the whole image from where the image values 
 * can be read.                                                               
 *  */
void Cordet::createNeighbMap(void){
    int x, y, imgPos, n = 0, stride = xImageSize - PsgMaskSize;
    imgPos = -halfPsgMaskSize * xImageSize - halfPsgMaskSize;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++, imgPos += stride)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++, imgPos++)
        neighbMap[n++] = imgPos;
}
/*
 * Creates the tables xs, ys.                                                  
 * */
void Cordet::createXsYsTables(void){
    int i, j, k = 0;
    for (i = -halfPsgMaskSize; i <= halfPsgMaskSize; i++)
      for (j = -halfPsgMaskSize; j <= halfPsgMaskSize; j++, k++)
        {xs[k] = j; ys[k] = i;}
}
/*
 * Precomputes the table of values of Wr(X) for all X in the mask.            
 * */
void Cordet::createWrTable(void){
    int x, y, sqr_y; float coef = -0.5f/(sigmaR*sigmaR), *tab = Wrs;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++) {
      sqr_y = y*y;
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++, tab++)
        *tab = exp(coef*(float)(sqr_y + x*x));
    }
}
/*
 * Computes dirXs table. Each item of this table contains the value of the    
 * diresction towards the corresponding point in the mask.                    
 * */
void Cordet::createDirXTable(void){
    int x, y, idx = 0;
    float dirX;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++)
        if ((x != 0)||(y != 0)){
          dirX = atan2((float)y,(float)x); if (dirX < 0) dirX += TWO_PIf;
          dirXs[idx++] = dirX;
        } else dirXs[idx++] = 0.0f;
}
/*
 * Computes the sizes and the directions of the gradient of brightness.       
 * Also, precomputes the values of sins and coss of the direction.            
 *  */
const float W1 = 0.10f; /* used masks */
const float W2 = 0.30f;

void Cordet::computeGrads(void){
    int i, x, y, imgPos; float *al, *pl, *nl, a, b, dx, dy, r, angle;
    
    for( i = 0; i < numImagePixs; i++ ) gSizes[i] = 0.0f;
    
    imgPos = xImageSize + 1;  /*  since we skip the first (and the last) line */
    pl = &image[1]; al = &image[xImageSize+1]; nl = &image[2*xImageSize+1];
    for (y = 1; y < yImageSize - 1; y++, imgPos +=2, pl+=2, al+=2, nl+=2)
      for (x = 1; x < xImageSize - 1; x++, imgPos++, pl++, al++, nl++){
        a = *(nl + 1) - *(pl - 1);
        b = *(nl - 1) - *(pl + 1);
        dx = W1*(a - b) + W2*(*(al + 1) - *(al - 1));
        dy = W1*(a + b) + W2*(     *nl  -      *pl );
        if ((r = dx*dx + dy*dy) > 0.001f ){
          angle = atan2(dy,dx); if (angle < 0) angle += TWO_PIf;
          gSizes[imgPos] = r = sqrt(r);
          gDirs[imgPos]  = angle;
          gDsins[imgPos] = (short)(pdTableScalef*dy/r);
          gDcoss[imgPos] = (short)(pdTableScalef*dx/r);
        }
      }
}
/*
 * Computes the square of Beaudet's DET that is used for estimating the noise.
 * */
void Cordet::computeDets(void){
    int i, x, y, imgPos; float *al, *pl, *nl;
    float a11,a12,a13, a21,a22,a23, a31,a32,a33;
    float r,s,t, det;
    
    for( i = 0; i < numImagePixs; i++ ) dets[i] = 0.0f;
    
    imgPos = xImageSize + 1;   /* since we skip the first (and the last) line */
    pl = &image[1]; al = &image[xImageSize+1]; nl = &image[2*xImageSize+1];
    for (y = 1; y < yImageSize - 1; y++, imgPos +=2, pl+=2, al+=2, nl+=2)
      for (x = 1; x < xImageSize - 1; x++, imgPos++, pl++, al++, nl++){
        if (gSizes[imgPos] <= noiseGradSizeThresh){
          a31 = *(nl-1); a32 = *nl; a33 = *(nl+1);
          a21 = *(al-1); a22 = *al; a23 = *(al+1);
          a11 = *(pl-1); a12 = *pl; a13 = *(pl+1);
          r = a21 - 2.0f*a22 + a23;
          s = 0.25f*(a11 + a33 - a31 - a13);
          t = a12 - 2.0f*a22 + a32;
          det = r*t - s*s;
          dets[imgPos] = det * det;
        }
      }
}

/* 
* This method estimate noise, respectively the sigma value
* */
float Cordet::estimateNoiseSigma(int imgPosQ){
    int   x, y, mapIdx = 0, cnt = 0, imgPos;
    float sum = 0.0f, meanDet, sigma, c = 8.0f;
    for (y = -halfPsgMaskSize; y <= halfPsgMaskSize; y++)
      for (x = -halfPsgMaskSize; x <= halfPsgMaskSize; x++){
        imgPos = imgPosQ + neighbMap[mapIdx++];
        if (gSizes[imgPos] <= noiseGradSizeThresh){sum += dets[imgPos]; cnt++;}
      }
   if (cnt) {
     meanDet = sqrt( sum / (float)(cnt));
     sigma = sqrt(meanDet / c);
     if (sigma > noiseGradSizeThresh) sigma = meanDet/(c * noiseGradSizeThresh);
     return sigma;
   } else return 0.0f;
}

/*
 * Precomputes the table of values of the probability density pd(z).          
 * */
void Cordet::createPdTable(void){
    int i; float *tab = pdTable;
    float coef = -0.5f/((pdTableScalef*pdTableScalef)*(sigmaD*sigmaD));
    for (i = 0; i < pdTableSize; i++, tab++) *tab = exp(coef*(float)(i*i));
}


void inline Cordet::send3(int x,int y,int s1,int s2,int s3,int &q, float c){
  int k = (y + halfPsgMaskSize)*PsgMaskSize + x + halfPsgMaskSize;
  influenceMap[k] = q;     
  influences[q++] = 3;
  influences[q] = k + s1;  
  intCoefs[q++] = c;
  influences[q] = k + s2;  
  intCoefs[q++] = c;
  influences[q] = k + s3;  
  intCoefs[q++] = 1.0f;
}

void inline Cordet::send2(int x,int y,int s1,int s2,int &q, float c1, float c2) {
  int k = (y + halfPsgMaskSize)*PsgMaskSize + (x) + halfPsgMaskSize;
  influenceMap[k] = q;     
  influences[q++] = 2;
  influences[q] = k + s1;  
  intCoefs[q++] = c1;
  influences[q] = k + s2;  
  intCoefs[q++] = c2;
}

/*
 * Precomputes the tables for organising computation and interpolating Psg(X).
 * */
void Cordet::createOrgTables(int halfMaskSize){
    const int numMaskPixels = PsgMaskSize * PsgMaskSize,
      E =  1, 
      NE =  1 - PsgMaskSize, 
      N = -PsgMaskSize, 
      NW = -1 - PsgMaskSize,
      W = -1, 
      SW =  PsgMaskSize - 1, 
      S =  PsgMaskSize, 
      SE =  PsgMaskSize + 1;
    
    int q = 0; 

    float c, c1, c2;

    for (int k = 0; k < numMaskPixels; k++) flagsProcess[k] = 0;
    for (int k = 0; k < numMaskPixels; k++) influenceMap[k] = 0;

    influences[q++] = 0;

    for (int i = 1; i < halfMaskSize; i++){
      c = 1.0f /(float)(i+1);
      send3( i, 0, SE, NE,  E, q, c);
      send3( i,-i,  E,  N, NE, q, c);
      send3( 0,-i, NE, NW,  N, q, c);
      send3(-i,-i,  N,  W, NW, q, c);
      send3(-i, 0, NW, SW,  W, q, c);
      send3(-i, i,  W,  S, SW, q, c);
      send3( 0, i, SW, SE,  S, q, c);
      send3( i, i,  S,  E, SE, q, c);
      for (int j = 1; j < i; j++){
        c1 = c*(float)(j+1); c2 = c*(float)(i-j+1);
        send2( i, j, SE, E, q, c1, c2);
        send2( i,-j, NE, E, q, c1, c2);
        send2( j,-i, NE, N, q, c1, c2);
        send2(-j,-i, NW, N, q, c1, c2);
        send2(-i,-j, NW, W, q, c1, c2);
        send2(-i, j, SW, W, q, c1, c2);
        send2(-j, i, SW, S, q, c1, c2);
        send2( j, i, SE, S, q, c1, c2);
      }
    }
}

/**
 * Normalize angle in -PI/2 to Pi/2
 * */
inline void Cordet::diffAngles(float a1,float a2,float &d){
	d = a1 - a2;
	if (d > PIf) d -= TWO_PIf; 
	else if (d < -PIf) d += TWO_PIf;
}

/*
 * Computes the values of Psg(X) for a point Q that is just being processed.  
 **/
int Cordet::createPsgMap(int imgPosQ){
    float bQ, gQ, estCorrDir, diffDirX, absDiffDirX, PsgX, newPsgX;
    int   mapPosX, imgPosX, dX, hX, numInfluences, influencePosX, k;
    int   idxProcess = 0, idxInsert = 0, pixCount = 0;

    bQ  = image[imgPosQ]; gQ  = gSizes[imgPosQ];

    estCorrDir = gDirs[imgPosQ];
    diffDirXs[PsgMaskCenter] = diffDirX = 0.0f; // here was the mistake

    Psgs[PsgMaskCenter] = PsgWrs[PsgMaskCenter] = 1.0f;
    corrDirs[imgPosQ] = estCorrDir;
    importantPixs[pixCount++] = PsgMaskCenter;

    Psgs[(tobeProcessed[idxInsert++] = PsgMaskCenter + 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = PsgMaskCenter - 1)] = 1.0f;
    k = PsgMaskCenter - PsgMaskSize;
    Psgs[(tobeProcessed[idxInsert++] = k - 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k    )] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k + 1)] = 1.0f;
    k = PsgMaskCenter + PsgMaskSize;
    Psgs[(tobeProcessed[idxInsert++] = k - 1)] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k    )] = 1.0f;
    Psgs[(tobeProcessed[idxInsert++] = k + 1)] = 1.0f;

    while (idxProcess < idxInsert){
      mapPosX = tobeProcessed[idxProcess++];
      flagsProcess[mapPosX] = 0;
      if ((PsgX = Psgs[mapPosX]) > SMALL_PROBABILITY){
        if (gSizes[imgPosX = imgPosQ+neighbMap[mapPosX]] > noiseGradSizeThresh){
          diffAngles(gDirs[imgPosX], estCorrDir, diffDirX);
          if (diffDirX >= 0.0f) absDiffDirX = diffDirX;
          else absDiffDirX = -diffDirX;
          if (absDiffDirX < angleVal2){
        	  dX=abs((int)(pdTableScale2f*(image[imgPosX]-bQ)/(gSizes[imgPosX]+gQ)));
            if (dX < pdTableSize)
            {
            hX = abs(xs[mapPosX]*gDcoss[imgPosX] + ys[mapPosX]*gDsins[imgPosX]);
              if (hX < pdTableSize)
              {
                newPsgX = pdTable[dX]*pdTable[hX];
                if (absDiffDirX > angleVal1)
                  newPsgX *= 1.0f - (absDiffDirX - angleVal1)*invAngleRampWidth;
                
                if (newPsgX > SMALL_PROBABILITY){
                  if (newPsgX < PsgX) PsgX = newPsgX;
                  k = influenceMap[mapPosX];
                  if ((numInfluences = influences[k]) > 0){
                    
                	if (!flagsProcess[influencePosX = influences[++k]]){
                      Psgs[influencePosX] = intCoefs[k]*PsgX;
                      tobeProcessed[idxInsert++] = influencePosX;
                      flagsProcess[influencePosX] = 1;
                    } else Psgs[influencePosX] += intCoefs[k]*PsgX;
                    
                    if (!flagsProcess[influencePosX = influences[++k]]){
                      Psgs[influencePosX] = intCoefs[k]*PsgX;
                      tobeProcessed[idxInsert++] = influencePosX;
                      flagsProcess[influencePosX] = 1;
                    } else Psgs[influencePosX] += intCoefs[k]*PsgX;
                    
                    if (numInfluences == 3)
                      Psgs[(tobeProcessed[idxInsert++]=influences[++k])] = PsgX;
                    
                  }
                  diffDirXs[mapPosX] = diffDirX;
                  Psgs[mapPosX] = PsgX;
                  PsgWrs[mapPosX] = PsgX*Wrs[mapPosX];
                  importantPixs[pixCount++] = mapPosX;
                }
              }
            }
          }
        }
      }
    }
    return pixCount;
}

/*
 * Computes the corner direction, corner angle, corner mean gradient size,    
 * and the corner area size for the corner candidate at Q.                    
 * */
void Cordet::measureCornerCandidate(int imgPosQ, float *variance,
                            float *apparence, float *corrDir,
                            float *meanGr, float *inertiaRadius){

    float sum1, sum1w, sum2, sum2w, sum3, diffDir, dir, dir1, dir2, cDir;
    float halfAngle, w, s, c, d, refAngleVal = halfAngleThresh/2.5f;
    int   i, j;

   *apparence = MIN_APPAR; *meanGr = *inertiaRadius = 0.0f;
    sum1 = sum1w = sum2 = 0.0f;
    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i]; w = PsgWrs[j];
      sum1w += w; sum1 += w*diffDirXs[j];
    }
    dir = sum1/sum1w;

    cDir = dir + corrDirs[imgPosQ];
    if (cDir > TWO_PIf) cDir -= TWO_PIf; else if(cDir < 0.0f ) cDir += TWO_PIf;
    *corrDir = cDir;

    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i];
      diffDir = diffDirXs[j] - dir; if (diffDir < 0.0f) diffDir = -diffDir;
      sum2 += diffDir*diffDir*PsgWrs[j];
    }
    
    *variance  = sum2/sum1w; 
    halfAngle = sqrt(*variance);
    if (halfAngle < halfAngleThresh) return;

    if (halfAngle < 0.4f){
      s = sin(cDir); 
      c = cos(cDir);
      sum1 = sum1w = sum2 = sum2w = 0.0f;
      for (i = 0; i < numImportantPixs; i++){
        j = importantPixs[i]; w = Psgs[j];
        d = -s*(float)(xs[j]) + c*(float)(ys[j]);
        if (d > 0) {sum1w += w; sum1 += w*diffDirXs[j];}
        if (d < 0) {sum2w += w; sum2 += w*diffDirXs[j];}
      }

      if (sum1w > 0.0f) dir1 = sum1/sum1w; else dir1 = 0.0f;
      if (sum2w > 0.0f) dir2 = sum2/sum2w; else dir2 = 0.0f;

      sum3 = 0.0f;
      
      for (i = 0; i < numImportantPixs; i++){
        j = importantPixs[i]; 
        w = Psgs[j];
        d = -s*(float)(xs[j]) + c*(float)(ys[j]);
        
        if (d > 0)
        {
        	diffDir = diffDirXs[j] - dir1; 
        	sum3 += w*diffDir*diffDir;
        }
        
        if (d < 0)
        {
        	diffDir = diffDirXs[j] - dir2; 
        	sum3 += w*diffDir*diffDir;
        }
      }//for
      refAngleVal = 0.7f*sqrt(sum3/(sum1w + sum2w));
    
    }

    sum1 = sum1w = sum2 = sum2w = sum3 = 0.0f;
    for (i = 0; i < numImportantPixs; i++){
      j = importantPixs[i];
      diffDir = diffDirXs[j] - dir; 
      
      if (diffDir < 0.0f) 
    	  diffDir = -diffDir;
      
      if (diffDir > refAngleVal)
      {
        w = gSizes[imgPosQ + neighbMap[j]] * Psgs[j];
        sum1w += w; 
        sum1 += w * (xs[j]*xs[j] + ys[j]*ys[j]);
        w = PsgWrs[j];
        sum2w += w; sum2 += w * gSizes[imgPosQ + neighbMap[j]];
      }
      sum3  += (diffDir - refAngleVal)*gSizes[imgPosQ + neighbMap[j]]*Psgs[j];
    }
    
    *apparence = sum3;
    if (sum1w > 0.0f){
    	*inertiaRadius = sqrt(sum1/sum1w); 
    	*meanGr = sum2/sum2w;
    }
}
/*
 * This function computes the values of corr(Q) and appar(Q) for all the image
 * points (except the points in the border of width halfMaskSize + 1).        
 * */
void Cordet::computeMeasuresAndApparences(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2 * first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    int       x, y, i, imgPosQ = first * xImageSize + first;
    float     variance, appar, dir, meanGr, inertiaRadius;

    for( i = 0; i < numImagePixs; i++ )  corrs[i] = 0.0f;
    for( i = 0; i < numImagePixs; i++ ) appars[i] = MIN_APPAR;
    
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (gSizes[imgPosQ] > noiseGradSizeThresh){
          numImportantPixs = createPsgMap(imgPosQ);
          if (numImportantPixs >= SMALL_NUM_PIXELS){

            measureCornerCandidate(imgPosQ, &variance, &appar,
                                   &dir, &meanGr, &inertiaRadius);

            if(inertiaRadius  < inertiaRadiusThresh) appar = MIN_APPAR;
            if(meanGr         <  meanGradSizeThresh) appar = MIN_APPAR;
            corrs[imgPosQ]    = variance * gSizes[imgPosQ];
            appars[imgPosQ]   = appar;
            corrDirs[imgPosQ] = dir;
            SNRs[imgPosQ]     = meanGr;
          }
        }
}
/*
 * This function decides whether the function corrs has its maximum at Q.     
 * */
short Cordet::isCorrMax(int imgPosQ){
    float corr = corrs[imgPosQ];
    if ((corr >= corrs[imgPosQ + 1])&&
        (corr >= corrs[imgPosQ - xImageSize + 1])&&
        (corr >= corrs[imgPosQ - xImageSize])&&
        (corr >= corrs[imgPosQ - xImageSize - 1])&&
        (corr >= corrs[imgPosQ - 1])&&
        (corr >= corrs[imgPosQ + xImageSize - 1])&&
        (corr >= corrs[imgPosQ + xImageSize])&&
        (corr >= corrs[imgPosQ + xImageSize + 1]))
          return 1;
    else  return 0;
}
/*
 * The function markCandidates selects the points at which corr() has its     
 * and apparency is greater than zero.                                        
 * */
void Cordet::markCandidates(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    int       x, y, imgPosQ;

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (appars[imgPosQ] > MIN_APPAR){
          isCandidate[imgPosQ] = isCorrMax(imgPosQ);
        } else isCandidate[imgPosQ] = 0;
}
/*
 * The function modifyApparency that follows modifies the value of appars(Q)
 * by multiplying it by a coefficient that may decrease the value of apprence 
 * of corner at Q. The coefficient is determined by examining the level of    
 * noise of the size of the gradient of brigtness in the neighbourhood of Q.  
 * */
void Cordet::modifyApparency(void){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    float     meanSNR, coef, logMeanSNR, sum = 0.0;
    int       x, y, imgPosQ, cnt = 0;

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (isCandidate[imgPosQ]){
          SNRs[imgPosQ] /= (estimateNoiseSigma(imgPosQ) + 0.01f*SNRs[imgPosQ]);
          sum += SNRs[imgPosQ]; cnt++;
        }

    if (!cnt) return;
    meanSNR = sum / (float)(cnt);
    logMeanSNR = log(meanSNR);

    imgPosQ = first * xImageSize + first;
    for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
      for (x = first; x <= last_x; x++, imgPosQ++)
        if (isCandidate[imgPosQ]){
          coef = log(SNRs[imgPosQ]) / logMeanSNR;
          appars[imgPosQ] *= coef;
        }
}
/*
 * This function checkes whether an another greater maximum of corrs() exists 
 * in the neighbourhood of Q (imgPosQ) whose radius is halfExtNeighbSize. If  
 * so, if the corner directions are similar at both the maxima, and if both   
 * the maxima are not separated by a low value of corrs() between them, the   
 * candidate will be rejected as a corner.                                    
 * */
int Cordet::checkCornerInGreaterNeighbourhood(int imgPosQ){
    int   mapPosX, imgPosX, idxProcess = 0, idxInsert = 0;
    int   numInfluences, influencePosX, k;
    float diffDirX, corrX, minSepX, *minSep = Psgs,
          corrQ = corrs[imgPosQ], corrDirQ = corrDirs[imgPosQ],
          halfAngleQ = (HALF_PIf - sqrt(corrQ/gSizes[imgPosQ]));

    minSep[(tobeProcessed[idxInsert++] = PsgMaskCenter + 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = PsgMaskCenter - 1)] = corrQ;
    k = PsgMaskCenter - PsgMaskSize;
    minSep[(tobeProcessed[idxInsert++] = k - 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k    )] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k + 1)] = corrQ;
    k = PsgMaskCenter + PsgMaskSize;
    minSep[(tobeProcessed[idxInsert++] = k - 1)] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k    )] = corrQ;
    minSep[(tobeProcessed[idxInsert++] = k + 1)] = corrQ;

    while (idxProcess < idxInsert){
      mapPosX = tobeProcessed[idxProcess++];
      imgPosX = imgPosQ + neighbMap[mapPosX];
      flagsProcess[mapPosX] = 0;
      minSepX = minSep[mapPosX];
      corrX   = corrs[imgPosX];

      if (corrX > corrQ){
        if (isCandidate[imgPosX]){
          diffAngles(corrDirQ, corrDirs[imgPosX], diffDirX);
          if (diffDirX < 0.0f) diffDirX = -diffDirX;
          if (diffDirX < halfAngleQ){
            diffAngles(corrDirQ, dirXs[mapPosX], diffDirX);
            if (diffDirX < 0.0f) diffDirX = -diffDirX;
            if ((PIf - diffDirX) < diffDirX) diffDirX = PIf - diffDirX;
            if (diffDirX < halfAngleQ){
              if ((minSepX*corrX)/(corrQ*corrQ) > 0.25f){
                while (idxProcess < idxInsert)
                  flagsProcess[tobeProcessed[idxProcess++]] = 0;
                return 0;
              }
            }
          }
        }
      }

      k = influenceMap[mapPosX];
      if ((numInfluences = influences[k]) > 0){
        if(corrX > 0.0f){
          diffAngles(corrDirQ, corrDirs[imgPosX], diffDirX);
          if (diffDirX < 0.0f) diffDirX = -diffDirX;
          if (diffDirX > halfAngleQ) corrX = 0.0f;
          if (corrX < minSepX) minSepX = corrX;
        } else minSepX = 0.0f;

        if (!flagsProcess[influencePosX = influences[++k]]){
          minSep[influencePosX] = intCoefs[k]*minSepX;
          tobeProcessed[idxInsert++] = influencePosX;
          flagsProcess[influencePosX] = 1;
        } else minSep[influencePosX] += intCoefs[k]*minSepX;
        if (!flagsProcess[influencePosX = influences[++k]]){
          minSep[influencePosX] = intCoefs[k]*minSepX;
          tobeProcessed[idxInsert++] = influencePosX;
          flagsProcess[influencePosX] = 1;
        } else minSep[influencePosX] += intCoefs[k]*minSepX;
        if (numInfluences == 3)
          minSep[(tobeProcessed[idxInsert++]=influences[++k])] = minSepX;
      }
    }
    return 1;
}
/*
 * The function decideCorners goes through the values of corr(Q) and appar(Q) 
 * and searches for the corners. A point Q is marked as a corner if corr(Q)   
 * has its maximum at Q and if appar(Q) is greater than a predefined          
 * threshold. The coordinates x,y of the corner points are written into       
 * the array "cornerList". The function returns the number of corners that    
 * were found. It returns -2 if the array is not big enough.                  
 * */
int Cordet::decideCorners(int *cornerList, const int cornerListLen){
    const int first  = halfPsgMaskSize + 1, doubleFirst = 2*first,
              last_x = xImageSize - halfPsgMaskSize - 2,
              last_y = yImageSize - halfPsgMaskSize - 2;
    int       x, y, imgPosQ, cornerListPos = 0;

    markCandidates();
    if (options & CHECK_APPAR_SNR) modifyApparency();

    if (halfExtMaskSize == 1){
      imgPosQ = first * xImageSize + first;
      for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
        for (x = first; x <= last_x; x++, imgPosQ++)
          if ((isCandidate[imgPosQ])&&(appars[imgPosQ] > apparenceThresh))
            {
        	  //add to cornerList
        	    if (cornerListPos > (cornerListLen - 2)) return -2;
        	    cornerList[cornerListPos++] = x; 
        	    cornerList[cornerListPos++] = y; 
            }
    } else {
      imgPosQ = first * xImageSize + first;
      for (y = first; y <= last_y; y++, imgPosQ += doubleFirst)
        for (x = first; x <= last_x; x++, imgPosQ++)
          if ((isCandidate[imgPosQ])&&(appars[imgPosQ] > apparenceThresh)){
            if (checkCornerInGreaterNeighbourhood(imgPosQ))
            {
            	//add to cornerList
            	if (cornerListPos > (cornerListLen - 2)) return -2;
          	    cornerList[cornerListPos++] = x; 
          	    cornerList[cornerListPos++] = y; 
            }
          }
    }
    return cornerListPos / 2;
}
/*
 * This function sorts the corners according to their apparence (obviousness).
 * */
void Cordet::sortCorners( int l, int r ){
    int i, j, c, tmp; float val;
      i = l; j = r;
      c = (l+r)/2; if (c & 1) c--;
      val = appars[corners[c] + corners[c+1]*xImageSize];
      do {
        while (appars[corners[i] + corners[i+1]*xImageSize] > val){i++; i++;}
        while (val > appars[corners[j] + corners[j+1]*xImageSize]){j--; j--;}
        if (i <= j){
          tmp= corners[i];   corners[i]  = corners[j];   corners[j]  = tmp;
          tmp= corners[i+1]; corners[i+1]= corners[j+1]; corners[j+1]= tmp;
          i++; i++; j--; j--;
        }
      } while (i <= j);
      if (l < j) sortCorners( l, j );
      if (i < r) sortCorners( i, r );
}
/*
 * This is the "main function" that performs the computation. If you want to  
 * use the algorithm, you will only call this function. You will find the     
 * detailed explanation of its parameters as well as practical advice how     
 * to set them in the file "cordet.h". You can also have a look at the test   
 * program in "cortest.cpp" and the corresponding script files to obtain some   
 * practical examples.                                                        
 * The function returns either the number of corners that were found or -1    
 * if there was not enough memory to solve the task or -2 if the array        
 * "cornerList" was not big enough for all the corners that were found.       
 */

int Cordet::detectCorners(
    float *image_p,
    int    xImageSize_p,
    int    yImageSize_p,
    int    halfPsgMaskSize_p,
    float  angleThresh_p,
    float  noiseGradSizeThresh_p,
    float  apparenceThresh_p,
    float  meanGradSizeThresh_p,
    float  inertiaRadiusThresh_p,
    float  sigmaD_p,
    float  sigmaR_p,
    int    halfExtMaskSize_p,
    int    options_p,
    int   *cornerList,
    int    cornerListLen)
{
/*----------------------------------------------------------------------------*/

    int numCorners; float tmp;

    image               = image_p;
    xImageSize          = xImageSize_p;
    yImageSize          = yImageSize_p;
    halfPsgMaskSize     = halfPsgMaskSize_p;
    angleThresh         = angleThresh_p;
    noiseGradSizeThresh = noiseGradSizeThresh_p;
    apparenceThresh     = apparenceThresh_p;
    meanGradSizeThresh  = meanGradSizeThresh_p;
    inertiaRadiusThresh = inertiaRadiusThresh_p;
    sigmaD              = sigmaD_p;
    sigmaR              = sigmaR_p;
    halfExtMaskSize     = halfExtMaskSize_p;
    options             = options_p;
    corners             = cornerList;
    numImagePixs        = xImageSize*yImageSize;
    PsgMaskSize         = 2*halfPsgMaskSize + 1;
    numPsgMaskPixs      = PsgMaskSize * PsgMaskSize;
    PsgMaskCenter       = PsgMaskSize * halfPsgMaskSize + halfPsgMaskSize;
    halfGradSizeThresh  = 0.5f*noiseGradSizeThresh;
    tenthGradSizeThresh = 0.1f*noiseGradSizeThresh;
    apparenceThresh    *= noiseGradSizeThresh;
    halfAngleThresh     = angleThresh / 2.0f;
    angleVal1           = HALF_PIf - ANGLE_RAMP_WIDTH;
    angleVal2           = HALF_PIf + 0.0f;
    invAngleRampWidth   = 1.0f / (angleVal2 - angleVal1);
    pdTableScale        = PD_TABLE_SCALE;
    pdTableScalef       = (float)pdTableScale;
    pdTableScale2f      = 2.0f*pdTableScalef;
    tmp                 = sqrt(-2.0f*sigmaD*sigmaD*log(SMALL_PROBABILITY));
    pdTableSize         = (int)(pdTableScalef*tmp) + 2;

    gSizes              = (float *)malloc(numImagePixs * sizeof(gSizes[0]));
    dets                = (float *)malloc(numImagePixs * sizeof(dets[0]));
    gDirs               = (float *)malloc(numImagePixs * sizeof(gDirs[0]));
    gDsins              = (short *)malloc(numImagePixs * sizeof(gDsins[0]));
    gDcoss              = (short *)malloc(numImagePixs * sizeof(gDcoss[0]));
    corrDirs            = (float *)malloc(numImagePixs * sizeof(corrDirs[0]));
    corrs               = (float *)malloc(numImagePixs * sizeof(corrs[0]));
    appars              = (float *)malloc(numImagePixs * sizeof(appars[0]));
    SNRs                = (float *)malloc(numImagePixs * sizeof(SNRs[0]));
    neighbMap           = (int *)malloc(numPsgMaskPixs  * sizeof(neighbMap[0]));
    Psgs                = (float *)malloc(numPsgMaskPixs  * sizeof(Psgs[0]));
    Wrs                 = (float *)malloc(numPsgMaskPixs  * sizeof(Wrs[0]));
    PsgWrs              = (float *)malloc(numPsgMaskPixs  * sizeof(PsgWrs[0]));
    xs                  = (int *)malloc(numPsgMaskPixs  * sizeof(xs[0]));
    ys                  = (int *)malloc(numPsgMaskPixs  * sizeof(ys[0]));
    dirXs               = (float *)malloc(numPsgMaskPixs  * sizeof(dirXs[0]));
    diffDirXs           = (float *)malloc(numPsgMaskPixs  * sizeof(diffDirXs[0]));
    flagsProcess        = (int *)malloc(numPsgMaskPixs  * sizeof(flagsProcess[0]));
    influenceMap        = (int *)malloc(numPsgMaskPixs  * sizeof(influenceMap[0]));
    influences          = (int *)malloc((4*numPsgMaskPixs) * sizeof(influences[0]));
    intCoefs            = (float *)malloc((4*numPsgMaskPixs) * sizeof(intCoefs[0]));
    tobeProcessed       = (int *)malloc(numPsgMaskPixs  * sizeof(tobeProcessed[0]));
    importantPixs       = (int *)malloc(numPsgMaskPixs  * sizeof(importantPixs[0]));
    pdTable             = (float *)malloc(pdTableSize     * sizeof(pdTable[0]));

    if ((gSizes != NULL)&&(dets != NULL)&&(gDirs != NULL)&&(gDsins != NULL)&&
        (gDcoss != NULL)&&(corrDirs != NULL)&&(corrs != NULL)&&(SNRs != NULL)&&
        (appars != NULL)&&(neighbMap != NULL)&&(Psgs != NULL)&&(Wrs != NULL)&&
        (PsgWrs != NULL)&&(xs != NULL)&&(ys != NULL)&&(dirXs != NULL)&&
        (diffDirXs != NULL)&&(flagsProcess != NULL)&&(influenceMap != NULL)&&
        (influences != NULL)&&(intCoefs != NULL)&&(tobeProcessed != NULL)&&
        (importantPixs != NULL)&&(pdTable != NULL)){

             isCandidate = gDsins;
             createPdTable();
             createWrTable();
             createNeighbMap();
             createXsYsTables();
             createOrgTables(halfPsgMaskSize);
             computeGrads();
             if (options & CHECK_APPAR_SNR) computeDets();
             computeMeasuresAndApparences();
             createOrgTables(halfExtMaskSize);
             createDirXTable();
             numCorners = decideCorners(cornerList, cornerListLen);
             if (options & SORT_CORNERS){
               if (numCorners > 1){
                 sortCorners(0, 2*(numCorners - 1));
               }
             }

    } else numCorners = -1;

    if (gSizes        != NULL) free(gSizes);
    if (dets          != NULL) free(dets);
    if (gDirs         != NULL) free(gDirs);
    if (gDsins        != NULL) free(gDsins);
    if (gDcoss        != NULL) free(gDcoss);
    if (corrDirs      != NULL) free(corrDirs);
    if (corrs         != NULL) free(corrs);
    if (appars        != NULL) free(appars);
    if (SNRs          != NULL) free(SNRs);
    if (neighbMap     != NULL) free(neighbMap);
    if (Psgs          != NULL) free(Psgs);
    if (Wrs           != NULL) free(Wrs);
    if (PsgWrs        != NULL) free(PsgWrs);
    if (xs            != NULL) free(xs);
    if (ys            != NULL) free(ys);
    if (dirXs         != NULL) free(dirXs);
    if (diffDirXs     != NULL) free(diffDirXs);
    if (flagsProcess  != NULL) free(flagsProcess);
    if (influenceMap  != NULL) free(influenceMap);
    if (influences    != NULL) free(influences);
    if (intCoefs      != NULL) free(intCoefs);
    if (tobeProcessed != NULL) free(tobeProcessed);
    if (importantPixs != NULL) free(importantPixs);
    if (pdTable       != NULL) free(pdTable);

    return numCorners;
}
/*============================================================================*/

